(*2d kneed biped with torso*)
(*with all output functions*)

SetDirectory["/home/shu/workspace/Research/2DKnee_Torso_Outputs"];
<< RobotLinks.m
<< Linearize.m
(*SetDirectory[NotebookDirectory[]<>"build_torso"];*)
(*choose the \
output combination*)
(*LineNumber = 1;*)
IndexAllPre = Import["data/IndexAll.mat", "MAT"];
(*Dimensions[IndexAllPre];*)
IndexAll = Join[First[IndexAllPre]];
(*Dimensions[IndexAll];*)

HipPosIndex = Round[IndexAll[[LineNumber, 1]]];
NSslopeIndex = Round[IndexAll[[LineNumber, 2]]];
TorsoIndex = Round[IndexAll[[LineNumber, 3]]];
(*constsubs={Lc->43561/100000,Lt->40134/100000,LT->3945/10000,mh->615/\
10,mt->91/10,mc->4218/1000,mf->1315/1000,g->981/100};*)
\
(*constsubs={Lc->43561/100000,Lt->4195/100000,LT->3751/10000,mh->615/\
10,mt->91/10,mc->4218/1000,mf->1315/1000,g->981/100};*)
constsubs = \
{Lc -> 3806/10000, Lt -> 4522/10000, LT -> 3273/10000, mh -> 471/10, 
  mt -> 694/100, mc -> 323/100, mf -> 1006/1000, 
  g -> 981/100};(*mean human model*)
ndof = 5;
mm = {mf, mc, mt, mh, mt, mc, mf} /. constsubs;
statesubs = 
  Join[Table[Subscript[\[Theta], i][t] -> x[i], {i, 1, ndof}], 
   Table[Derivative[1][Subscript[\[Theta], i]][t] -> x[i + ndof], {i, 
     1, ndof}]];
(*mm={mf,mc,mt,mh,mt,mc,mf}/.constsubs;*)

p0 = {Subscript[p, x][t] -> 0, Subscript[p, z][t] -> 0, 
   Subscript[p, x]'[t] -> 0, Subscript[p, z]'[t] -> 0};
q = Table[{Subscript[\[Theta], i][t]}, {i, 1, ndof}];
dq = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]q\);
ddq = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]dq\);
qe = Join[{{Subscript[p, x][t]}, {Subscript[p, z][t]}}, q];
dqe = \!\(
\*SubscriptBox[\(\[PartialD]\), \(t\)]qe\);
(*location and direction of twists*)(*positive z direction*)

Subscript[\[Xi], 0] = {0, 0, 0, 0, 0, 0};
Subscript[\[Xi], px] = PrismaticTwist[{0, 0, 0}, {1, 0, 0}];
Subscript[\[Xi], pz] = PrismaticTwist[{0, 0, 0}, {0, 0, 1}];
Subscript[\[Xi], q1] = RevoluteTwist[{0, 0, 0}, {0, -1, 0}];
Subscript[\[Xi], q2] = RevoluteTwist[{0, 0, Lc}, {0, -1, 0}];
Subscript[\[Xi], q3] = RevoluteTwist[{0, 0, Lc + Lt}, {0, -1, 0}];
Subscript[\[Xi], q4] = RevoluteTwist[{0, 0, Lc + Lt}, {0, 1, 0}];
Subscript[\[Xi], q5] = RevoluteTwist[{0, 0, Lc}, {0, 1, 0}];
(*base configuration*)

Subscript[g, Subscript[sl, 1]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}];
Subscript[g, Subscript[sl, 2]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, (1 - 433/1000) Lc}];
Subscript[g, Subscript[sl, 3]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + (1 - 433/1000) Lt}];
Subscript[g, Subscript[sl, 4]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + Lt + LT}];
Subscript[g, Subscript[sl, 5]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + (1 - 433/1000) Lt}];
Subscript[g, Subscript[sl, 6]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, (1 - 433/1000) Lc}];
Subscript[g, Subscript[sl, 7]][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}];
(*calculate the forward kinematics maps*)

Subscript[g, 1][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], Subscript[p, z][t]}, 
    Subscript[g, Subscript[sl, 1]][0]]];
Subscript[g, 2][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, Subscript[g, Subscript[sl, 2]][0]]];
Subscript[g, 3][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, Subscript[g, Subscript[sl, 3]][0]]];
Subscript[g, 4][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], Subscript[p, z][t]},
     {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, Subscript[g, Subscript[sl, 4]][0]]];
Subscript[g, 5][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, Subscript[g, Subscript[sl, 5]][0]]];
Subscript[g, 6][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 6]][0]]];
Subscript[g, 7][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
     Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
     Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, Subscript[sl, 7]][0]]];
Subscript[g, stf][0] = RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}];
Subscript[g, stk][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc}];
Subscript[g, hip][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + Lt}];
Subscript[g, torso][0] = 
  RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc + Lt + LT}];
Subscript[g, nsk][0] = RPToHomogeneous[IdentityMatrix[3], {0, 0, Lc}];
 Subscript[g, nsf][0] = 
 RPToHomogeneous[IdentityMatrix[3], {0, 0, 0}];
Subscript[g, stf][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], 0], 0}, 
    Subscript[g, stf][0]]];
Subscript[g, stk][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, Subscript[g, stk][0]]];
Subscript[g, hip][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, Subscript[g, hip][0]]];
Subscript[g, torso][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, Subscript[g, torso][0]]];
Subscript[g, nsk][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, Subscript[g, nsk][0]]];
Subscript[g, nsf][\[Theta]] = 
  Simplify[ForwardKinematics[{Subscript[\[Xi], q1], 
     Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
     Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
     Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
     Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
     Subscript[\[Theta], 5][t]}, Subscript[g, nsf][0]]];
(*aniplot*)

pos = Simplify[
   Join[Subscript[g, stf][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, stk][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, hip][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, torso][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, hip][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, nsk][\[Theta]][[{1, 3}, {4}]], 
     Subscript[g, nsf][\[Theta]][[{1, 3}, {4}]], 2] /. constsubs];
(*calculate center of mass*)
Subscript[p, COM] = (\!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[
        i]]\ \(
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]\)[[1, 
         4]]/\((2*\((mf + mc + mt)\) + mh)\)\)\)) //. p0 // Simplify;
(*calculate the kinetic energy and manipulator inertia matrix*)

For[i = 1, i <= Length[mm], i++, Subscript[v, i] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({t, 1}\)]\(RigidPosition[
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]]\)\) /. constsubs]];
T = 1/2 \!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[
      i]]\ 
\*SubscriptBox[\(v\), \(i\)] . 
\*SubscriptBox[\(v\), \(i\)]\)\);
\[ScriptCapitalD]e = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[dqe], 2}\)]T\) /. p0];
(*project out the generalized coordinates defining the position of \
the stance foot to obtain the reduced \[ScriptCapitalD] matrix*)
\
\[ScriptCapitalD] = 
  Simplify[\[ScriptCapitalD]e[[3 ;; All, 3 ;; All]] /. p0];
(*calculate Coriolis matrix*)
\[ScriptCapitalC] = 
  Simplify[InertiaToCoriolis[\[ScriptCapitalD], Flatten[q], 
    Flatten[dq]]];
(*calculate the potential energy and \[ScriptCapitalG] matrix*)

V = Simplify[g \!\(
\*UnderoverscriptBox[\(\[Sum]\), \(i = 1\), \(Length[mm]\)]\(mm[[
        i]]\ \(
\(\*SubscriptBox[\(g\), \(i\)]\)[\[Theta]]\)[[3, 4]]\)\) /. constsubs];
\[ScriptCapitalG] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({q, 1}\)]V\)];
(*calculate the \[ScriptCapitalE] matrix for impact and the guard*)

Subscript[g, nsf][0] = 
 RPToHomogeneous[
  IdentityMatrix[3], {0, 0, 0}];(*the position of swing foot*)

Subscript[g, nsf][\[Theta]] = 
 Simplify[ForwardKinematics[{Subscript[\[Xi], px], 
    Subscript[p, x][t]}, {Subscript[\[Xi], pz], 
    Subscript[p, z][t]}, {Subscript[\[Xi], q1], 
    Subscript[\[Theta], 1][t]}, {Subscript[\[Xi], q2], 
    Subscript[\[Theta], 2][t]}, {Subscript[\[Xi], q3], 
    Subscript[\[Theta], 3][t]}, {Subscript[\[Xi], q4], 
    Subscript[\[Theta], 4][t]}, {Subscript[\[Xi], q5], 
    Subscript[\[Theta], 5][t]}, Subscript[g, nsf][0]]];
\[ScriptCapitalE] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[qe], 1}\)]\(\(
\(\*SubscriptBox[\(g\), \(nsf\)]\)[\[Theta]]\)[[{1, 3}, 4]]\)\)] /. 
   constsubs;
\[ScriptCapitalE] /. {Subscript[\[Theta], 1][t] -> .2345, 
   Subscript[\[Theta], 2][t] -> .1894, 
   Subscript[\[Theta], 3][t] -> -.293, 
   Subscript[\[Theta], 4][t] -> .094, 
   Subscript[\[Theta], 4][t] -> -.210, Subscript[\[Theta], 4] -> .923};
h = Simplify[Subscript[g, nsf][\[Theta]][[3, 4]] /. p0 /. constsubs];
hdot =  Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
     h]\)\)];
(*torso angle*)
theta4 = 
 Subscript[\[Theta], 4][
  t];(*1 Torso non-stance thigh angle*)
TorsoHipAngle = 
 Subscript[\[Theta], 1][t] + Subscript[\[Theta], 2][t] + 
  Subscript[\[Theta], 3][t];(*2 Torso Hip angle*)
nstorso = (
 Subscript[g, nsf][\[Theta]][[1, 4]] - 
  Subscript[g, torso][\[Theta]][[1, 4]])/(
 Subscript[g, nsf][\[Theta]][[3, 4]] - 
  Subscript[g, torso][\[Theta]][[3, 
   4]]);(*3 Torso Non-stance Slope*)
LinearNStorso = 
 Linearize[nstorso /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
  Table[0, {i, 
    5}]];(*4 Linearized torso non-stance slope angle*)
\
(*LinearNStorsoPre=Normal[Series[(Subscript[g, \
nsf][\[Theta]][[1,4]]-Subscript[g, \
torso][\[Theta]][[1,4]])/(Subscript[g, \
nsf][\[Theta]][[3,4]]-Subscript[g, \
torso][\[Theta]][[3,4]])/.p0,{Subscript[\[Theta], \
1][t],0,1},{Subscript[\[Theta], 2][t],0,1},{Subscript[\[Theta], \
3][t],0,1},{Subscript[\[Theta], 4][t],0,1},{Subscript[\[Theta], \
5][t],0,1}]];(*Linearized torso non-stance slope angle*)
pt1 = \
Collect[LinearNStorsoPre,Subscript[\[Theta], 1][t]]/.{Subscript[\
\[Theta], 2][t]-> 0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], \
4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt2 = Collect[LinearNStorsoPre,Subscript[\[Theta], \
2][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 3][t]-> \
0,Subscript[\[Theta], 4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt3 = Collect[LinearNStorsoPre,Subscript[\[Theta], \
3][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt4 = Collect[LinearNStorsoPre,Subscript[\[Theta], \
4][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt5 = Collect[LinearNStorsoPre,Subscript[\[Theta], \
5][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], 4][t]-> 0};
LinearNStorso = pt1+pt2+pt3+pt4+pt5*)

storso = Subscript[g, torso][\[Theta]][[1, 4]]/
  Subscript[g, torso][\[Theta]][[3, 4]];
(*5 Torso stance slope*)
LinearStorso = 
 Linearize[storso /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
  Table[0, {i, 
    5}]];(*6 Linearized Torso stance slope*)
\
(*LinearStorsoPre=Normal[Series[Subscript[g, \
torso][\[Theta]][[1,4]]/Subscript[g, \
torso][\[Theta]][[3,4]]/.p0,{Subscript[\[Theta], \
1][t],0,1},{Subscript[\[Theta], 2][t],0,1},{Subscript[\[Theta], \
3][t],0,1},{Subscript[\[Theta], 4][t],0,1},{Subscript[\[Theta], \
5][t],0,1}]];(*Linearized Torso stance slope*)
pt1 = \
Collect[LinearStorsoPre,Subscript[\[Theta], 1][t]]/.{Subscript[\
\[Theta], 2][t]-> 0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], \
4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt2 = Collect[LinearStorsoPre,Subscript[\[Theta], 2][t]]/.{Subscript[\
\[Theta], 1][t]-> 0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], \
4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt3 = Collect[LinearStorsoPre,Subscript[\[Theta], 3][t]]/.{Subscript[\
\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> 0,Subscript[\[Theta], \
4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt4 = Collect[LinearStorsoPre,Subscript[\[Theta], 4][t]]/.{Subscript[\
\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> 0,Subscript[\[Theta], \
3][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt5 = Collect[LinearStorsoPre,Subscript[\[Theta], 5][t]]/.{Subscript[\
\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> 0,Subscript[\[Theta], \
3][t]-> 0,Subscript[\[Theta], 4][t]-> 0};
LinearStorso= pt1+pt2+pt3+pt4+pt5*)
(*non-stance slope*)
nsslope = (
  Subscript[g, nsf][\[Theta]][[1, 4]] - 
   Subscript[g, hip][\[Theta]][[1, 4]])/(
  Subscript[g, nsf][\[Theta]][[3, 4]] - 
   Subscript[g, hip][\[Theta]][[3, 4]]) /. 
  p0;(*1 non-stance slope*)
LinearNSslope = 
 Linearize[nsslope /. p0, Table[Subscript[\[Theta], i][t], {i, 5}], 
  Table[0, {i, 
    5}]];(*linearized non-stance slope*)
\
(*LinearNSslopePre=Simplify[Normal[Series[(Subscript[g, \
nsf][\[Theta]][[1,4]]-Subscript[g, \
hip][\[Theta]][[1,4]])/(Subscript[g, \
nsf][\[Theta]][[3,4]]-Subscript[g, \
hip][\[Theta]][[3,4]])/.p0,{Subscript[\[Theta], \
1][t],0,1},{Subscript[\[Theta], 2][t],0,1},{Subscript[\[Theta], \
3][t],0,1},{Subscript[\[Theta], 4][t],0,1},{Subscript[\[Theta], \
5][t],0,1}]]];(*linearized non-stance slope*)
pt1 = \
Collect[LinearNSslopePre,Subscript[\[Theta], 1][t]]/.{Subscript[\
\[Theta], 2][t]-> 0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], \
4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt2 = Collect[LinearNSslopePre,Subscript[\[Theta], \
2][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 3][t]-> \
0,Subscript[\[Theta], 4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt3 = Collect[LinearNSslopePre,Subscript[\[Theta], \
3][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 4][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt4 = Collect[LinearNSslopePre,Subscript[\[Theta], \
4][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], 5][t]-> 0};
pt5 = Collect[LinearNSslopePre,Subscript[\[Theta], \
5][t]]/.{Subscript[\[Theta], 1][t]-> 0,Subscript[\[Theta], 2][t]-> \
0,Subscript[\[Theta], 3][t]-> 0,Subscript[\[Theta], 4][t]-> 0};
LinearNSslope= pt1+pt2+pt3+pt4+pt5;*)
HipAngle = 
 Subscript[\[Theta], 3][t] - 
  Subscript[\[Theta], 4][t];(*hip angle*)
HipPos = 
 Subscript[g, hip][\[Theta]][[1, 4]] /. p0;
(*Hip Position*)
(*LHipPos = -Lc Subscript[\[Theta], \
1][t]-Lt(Subscript[\[Theta], 1][t]+Subscript[\[Theta], 2][t]);*)

LHipPos = 
  Linearize[HipPos, Table[Subscript[\[Theta], i][t], {i, 5}], 
   Table[0, {i, 
     5}]];(*Linearized Hip Position*)
(*-Subscript[\[Theta], \
1][t]-(Subscript[\[Theta], 1][t]+Subscript[\[Theta], \
2][t]);*)(*Dimensionless Linearized Hip Position*)
(*feedback control*)
\
\[Chi] = Join[q, dq];
d\[Chi] = D[\[Chi], t];
(*Hip Position Output:1. hip position;2. Linearized hip position 3. \
Dimensionless Linearized hip position*)

hipOutput = {HipPos, LHipPos};
Subscript[p, hip] = hipOutput[[HipPosIndex]] ;
Subscript[p, hipdot]  = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
\*SubscriptBox[\(p\), \(hip\)]]\)\)];
Subscript[v, hip] = D[Subscript[p, hip], t] // Simplify;
fb = {\[Sigma][t] -> (Subscript[p, hip] - p[1])/
   a[1, 1]};(*time-invariant parameterization:*)
\[Sigma]y = \[Sigma][
   t] /. fb;
HumanFunction[i_] := (
  a[i, 1] Cos[a[i, 2] \[Sigma][t]] + 
   a[i, 3] Sin[a[i, 2] \[Sigma][t]])/Exp[a[i, 4] \[Sigma][t]] + 
  a[i, 5];(*calculate Subscript[y, d]and its derivatives*)

Subscript[y, d, 1] = a[1, 1];
Subscript[y, d, 2] = 
  Transpose[{Table[HumanFunction[i], {i, 2, ndof}]}] /. fb;
Subscript[Dy, d, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(d, 1\)]]\)\)];
Subscript[Dy, d, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(d, 2\)]]\)\)];
Subscript[DLfy, d, 1] = \!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(d, 1\)] . d\[Chi]]\)\) // Simplify;
Subscript[DLfy, d, 2] = \!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(d, 2\)] . d\[Chi]]\)\);
(*calculate actual kinematics outputs and Jacobians*)

Subscript[y, a, 1] = D[Subscript[p, hip], t];
NSslopeOutput = {nsslope, LinearNSslope, HipAngle};
 
TorsoOutput = {TorsoHipAngle, theta4, nstorso, LinearNStorso, storso, 
   LinearStorso};
Subscript[y, a, 2] = 
  Simplify[{{NSslopeOutput[[NSslopeIndex]]}, {Subscript[\[Theta], 2][
        t]}, {Subscript[\[Theta], 5][
        t]}, {TorsoOutput [[TorsoIndex]]}} /. constsubs /. p0];
Subscript[Dy, a, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(a, 1\)]]\)\)];
Subscript[Dy, a, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(a, 2\)]]\)\)];
Subscript[DLfy, a, 1] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(a, 1\)] . d\[Chi]]\)\)];
Subscript[DLfy, a, 2] = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[\[Chi]], 1}\)]\(Flatten[
\*SubscriptBox[\(Dy\), \(a, 2\)] . d\[Chi]]\)\)];

(*zero dynamics*)
(*Zstatesubs = {Subscript[z, 1][t] -> z[1], 
  Subscript[z, 2][t] -> z[2]};
c = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
\*SubscriptBox[\(p\), \(hip\)]]\)\) /. constsubs];
H = Simplify[\!\(
\*SubscriptBox[\(\[PartialD]\), \({Flatten[q], 1}\)]\(Flatten[
\*SubscriptBox[\(y\), \(a, 2\)]]\)\) /. constsubs];
Phi=Join[{c},H];
fbz={\[Sigma][t]->(Subscript[z, 1][t]-p[1])/a[1,1]}
Subscript[yz,d,2]=Transpose[{Table[HumanFunction[i],{i,2,ndof}]}]/.\
fbz
PhiI=Simplify[Inverse[Phi].Join[{{Subscript[z,1][t]}},Subscript[yz,d,\
2]]];
PhidotI=Simplify[Inverse[Phi].Join[{{Subscript[z,2][t]}},D[Subscript[\
yz,d,2],Subscript[z,1][t]]*Subscript[z,2][t]]];*)
(*write the control \
files to disk*)

SetDirectory[
  "/home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso"];
stream = OpenWrite["h_dot_mat"];
Write[stream, hdot /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["jpos_mat"];
Write[stream, pos /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["D_mat"];
Write[stream, \[ScriptCapitalD] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["C_mat"];
Write[stream, \[ScriptCapitalC] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["G_vec"];
Write[stream, \[ScriptCapitalG] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["De_mat"];
Write[stream, \[ScriptCapitalD]e /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["E_mat"];
Write[stream, \[ScriptCapitalE] /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["h_sca"];
Write[stream, h /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["sigma_sca"];
Write[stream, \[Sigma]y /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["phip_dot_mat"];
Write[stream, Subscript[p, hipdot] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["phip_sca"];
Write[stream, Subscript[p, hip] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["pcom_sca"];
Write[stream, Subscript[p, COM] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["yd1_sca"];
Write[stream, Subscript[y, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["yd2_vec"];
Write[stream, Subscript[y, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dyd1_mat"];
Write[stream, Subscript[Dy, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dyd2_mat"];
Write[stream, Subscript[Dy, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfyd1_mat"];
Write[stream, Subscript[DLfy, d, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfyd2_mat"];
Write[stream, Subscript[DLfy, d, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["ya1_sca"];
Write[stream, Subscript[y, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["ya2_vec"];
Write[stream, Subscript[y, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dya1_mat"];
Write[stream, Subscript[Dy, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["Dya2_mat"];
Write[stream, Subscript[Dy, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfya1_mat"];
Write[stream, Subscript[DLfy, a, 1] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
stream = OpenWrite["DLfya2_mat"];
Write[stream, Subscript[DLfy, a, 2] /. constsubs /. statesubs];
Close[stream];
Clear[stream];
(*perl*)
SetDirectory[
  "/home/shu/workspace/Research/2DKnee_Torso_Outputs"];
Run["perl math2mat_torso_1.pl"];
SetDirectory[
  "/home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso"];
